﻿/*
 * lane_kalman.cpp
 *
 *  Created on: May 9, 2018
 *      Author: geoff
 * *
 */

#include "lane_kalman.h"
#include "calculate_tools.h"

namespace rmd {

const int CKalman::kStableTime = 2;
const int CKalman::kQualityTime = 2;
const int CKalman::kLinLooseMaxTime = 7;
const int CKalman::kContinuGood = 100;

CKalman::CKalman() {}

CKalman::~CKalman() {}

CKalman* CKalman::instance_ = NULL;
CKalman* CKalman::Instance() {
  if (instance_ == NULL) {
    instance_ = new CKalman();
    instance_->EkfInitialization();
  }
  return instance_;
}

std::vector<LaneResult> CKalman::Update(DataFrame* data_frame,
                                        std::vector<LaneLine>& now_lines) {
  data_frame_ = data_frame;

  std::vector<LaneResult> filter_lines;

  lane_width = data_frame->refine_lane_width;

  //  for (int i = 0; i < kNumEkf; i++) {
  //    line_ekf_[i].flag_obsev = false;
  //  }

  for (int i = 0; i < now_lines.size(); i++) {
    EkfProcessSingleLine(now_lines[i]);
  }

  for (int i = 0; i < kNumEkf; i++) {
    if (line_ekf_[i].flag_work) {
      TrackMainProcess(line_ekf_[i]);
    }
  }

  //向外传值
  for (int i = 0; i < kNumEkf; i++) {
    if (line_ekf_[i].flag_work == true) {
      //把需要的几个状态重置
      line_ekf_[i].nQuality = 0;
      line_ekf_[i].bIsTrue = false;

      //提出ekf的几个状态标志 由低到高
      if (line_ekf_[i].flag_unconfirm == true)
        line_ekf_[i].nQuality = 0;  //不可信
      if (line_ekf_[i].flag_obsev == true)
        line_ekf_[i].nQuality = 1;  //能够观察到
      if (line_ekf_[i].flag_is_good == true)
        line_ekf_[i].nQuality = 2;  //已经可以连续观测，可以进行输出

      if (line_ekf_[i].flag_stable == true)
        line_ekf_[i].nQuality = line_ekf_[i].time_continu_good + 100;

      if (line_ekf_[i].flag_obsev == true) line_ekf_[i].bIsTrue = true;

      LaneResult lane_result;

      lane_result.angle[0] = line_ekf_[i].Line.angle[0];
      lane_result.angle[1] = line_ekf_[i].Line.angle[1];
      lane_result.nDistance[0] = line_ekf_[i].Line.nDistance[0];
      lane_result.nDistance[1] = line_ekf_[i].Line.nDistance[1];
      lane_result.Line_current[0] = line_ekf_[i].Line_current[0];
      lane_result.Line_current[1] = line_ekf_[i].Line_current[1];
      lane_result.Line_current[2] = line_ekf_[i].Line_current[2];
      lane_result.Line_current[3] = line_ekf_[i].Line_current[3];

      lane_result.Line_now[0] = line_ekf_[i].Line_now[0];
      lane_result.Line_now[1] = line_ekf_[i].Line_now[1];
      lane_result.Line_now[2] = line_ekf_[i].Line_now[2];
      lane_result.Line_now[3] = line_ekf_[i].Line_now[3];

      lane_result.Line_pre[0] = line_ekf_[i].Line_pre[0];
      lane_result.Line_pre[1] = line_ekf_[i].Line_pre[1];
      lane_result.Line_pre[2] = line_ekf_[i].Line_pre[2];
      lane_result.Line_pre[3] = line_ekf_[i].Line_pre[3];

      lane_result.nIndex = line_ekf_[i].Line.nIndex;
      lane_result.nQuality = line_ekf_[i].nQuality;
      lane_result.bIsTrue = line_ekf_[i].bIsTrue;

      filter_lines.push_back(lane_result);
    }

    //保证下次滤波前的状态是重置的
    line_ekf_[i].flag_obsev = false;
  }

  return filter_lines;
}

void CKalman::EkfInitialization() {
  for (int i = 0; i < kNumEkf; i++) {
    line_ekf_[i].index = -1;
    line_ekf_[i].flag_obsev = false;
  }
}

void CKalman::TrackMainProcess(Track_Para& track_para) {
  if (track_para.flag_obsev == true) {
    track_para.time_no_obsev = 0;       //持续不可见计数清零
    track_para.time_continu_obsev++;    //持续观测计数累加
    track_para.flag_unconfirm = false;  //不稳定的标志位重置

    if (track_para.time_continu_obsev >= kEkfGoodTime) {
      track_para.flag_is_good = true;
    }

    if (track_para.flag_is_good == true) {
      track_para.time_continu_good++;
      if (track_para.time_continu_good >= kContinuGood)
        track_para.time_continu_good = kContinuGood;  //防止越界

      if (track_para.time_continu_good >= kStableTime) {
        //当足够好的时候，就设为稳定。如果中间有一帧没有观测到，后来又观测到了，因为flag_is_good和time_continu_good没有
        //置位的原因，很快就能够达到true的状态。
        track_para.flag_stable = true;
      }
    }
    //表征是否是首次观测，如果是则进入滤波器初始化状态
    if (track_para.flag_first == true) {
      track_para.flag_first = false;
      track_para.flag_unconfirm = false;  //不可信置为false，认为是可信的
      track_para.flag_stable = false;  //稳定置为false， 认为是不稳定的
      track_para.flag_is_good = false;   //表征滤波器输出值是否可信
      track_para.time_continu_good = 0;  //用于计数可用观测的次数
      SetFristObsevPara(track_para);
    } else  //如果不是则进入持续滤波状态
    {
      SetUpdatePara(track_para);
    }
  } else {
    track_para.time_no_obsev++;
    track_para.time_continu_obsev = 0;

    //如果持续BadTime次观测不到，就认为输出值不可信，没有达到这一条件，下面的值是不变的。
    if (track_para.time_no_obsev >= kEkfBadTime) {
      track_para.flag_stable = false;
      track_para.flag_is_good = false;
      track_para.time_continu_good = 0;
      track_para.flag_unconfirm = true;
    }

    //如果持续CancelTime次观测不到，将滤波器停止，同时将首次观测置为true，下一次观测来了就重启滤波器。
    if (track_para.time_no_obsev >= kEkfCancelTime) {
      track_para.flag_work = false;
      track_para.flag_first = true;
    }
  }
}

void CKalman::EkfProcessSingleLine(LaneLine& lane_line) {
  bool id_new_add = true;
  int id_index = -1;
  for (int i = 0; i < kNumEkf; i++) {
    //判断滤波器序列中是否有对应的线
    if (line_ekf_[i].flag_work && lane_line.nIndex == line_ekf_[i].index) {
      id_index = i;
      id_new_add = false;
      break;
    }
  }

  if (id_index == -1) {
    for (int i = 0; i < kNumEkf; i++) {
      if (line_ekf_[i].flag_work == false) {
        id_index = i;
        line_ekf_[i].flag_work = true;
        break;
      }
    }
    if (id_index == -1) id_index = kNumEkf - 1;
  }

  line_ekf_[id_index].Line = lane_line;
  line_ekf_[id_index].index = lane_line.nIndex;
  line_ekf_[id_index].flag_obsev = true;
  line_ekf_[id_index].Line_current[0] = lane_line.near_field[2];
  line_ekf_[id_index].Line_current[1] = lane_line.near_field[3];
  line_ekf_[id_index].Line_current[2] = lane_line.middle_field[2];
  line_ekf_[id_index].Line_current[3] = lane_line.middle_field[3];
}

void CKalman::SetFristObsevPara(Track_Para& track_para) {
  track_para.Line_now[0] = track_para.Line_current[0];
  track_para.Line_now[1] = track_para.Line_current[1];
  track_para.Line_now[2] = track_para.Line_current[2];
  track_para.Line_now[3] = track_para.Line_current[3];

  track_para.Line_pre[0] = track_para.Line_current[0];
  track_para.Line_pre[1] = track_para.Line_current[1];
  track_para.Line_pre[2] = track_para.Line_current[2];
  track_para.Line_pre[3] = track_para.Line_current[3];
}

void CKalman::SetUpdatePara(Track_Para& track_para) {
  track_para.Line_now[0] =
      GetCenterPoint(track_para.Line_pre[0], track_para.Line_current[0]);
  track_para.Line_now[1] =
      GetCenterPoint(track_para.Line_pre[1], track_para.Line_current[1]);
  track_para.Line_now[2] =
      GetCenterPoint(track_para.Line_pre[2], track_para.Line_current[2]);
  track_para.Line_now[3] =
      GetCenterPoint(track_para.Line_pre[3], track_para.Line_current[3]);

  track_para.Line_pre[0] = track_para.Line_current[0];
  track_para.Line_pre[1] = track_para.Line_current[1];
  track_para.Line_pre[2] = track_para.Line_current[2];
  track_para.Line_pre[3] = track_para.Line_current[3];
}

cv::Point CKalman::GetCenterPoint(cv::Point pt1, cv::Point pt2) {
  return cv::Point((pt1.x + pt2.x) >> 1, (pt1.y + pt2.y) >> 1);
}

void CKalman::ExcludeOutKalmanLinesByCurrent(
    std::vector<LaneResult>& tempResPriSaveLine) {
  int nSize = tempResPriSaveLine.size();
  if (nSize < 2) return;

  int nLeftIndex = -1;
  int nRightIndex = -1;
  int nLeftValue = -1;
  int nRightValue = -1;

  for (int i = 0; i < nSize; i++) {
    if (tempResPriSaveLine[i].nIsCurLane == 1) {
      nLeftIndex = tempResPriSaveLine[i].nIndex;
      nLeftValue = tempResPriSaveLine[i].Line_now[0].x;
    }
    if (tempResPriSaveLine[i].nIsCurLane == 2) {
      nRightIndex = tempResPriSaveLine[i].nIndex;
      nRightValue = tempResPriSaveLine[i].Line_now[0].x;
    }
  }

  //  fittingKalmanResult();
  //  g_vecResPriSaveLine.clear();
  //  g_vecResPriSaveLine = CKalman::Instance()->push_EKF_Result();
}
}
